#include <GUIConstants.au3>
#include <GUIConstantsEx.au3>
#include <WindowsConstants.au3>
#include <IE.au3>

AutoItSetOption ("CaretCoordMode", 1)

Global Const $history = 3
$count = 0
$updateLeft = false
$updateRight = false

$joy = _JoyInit()
$x = 0
$y = 0
$z = 0
$buttonPress = False
$locked = False

Dim $leftLabel[19]
Dim $rightLabel[19]
Dim $leftIndicator[19]
Dim $rightIndicator[19]
Dim $leftActive[19] = [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1]
Dim $rightActive[19] = [1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1]
Dim $oldLeft[$history + 1]
Dim $oldRight[$history + 1]

Dim $inactiveColor[19] = [0x004400, 0x004400, 0x004400, 0x444400, 0x444400, 0x444400, 0x440000, 0x440000, 0x440000, 0x000000, 0x440000, 0x440000, 0x440000,  0x444400, 0x444400, 0x444400, 0x004400, 0x004400, 0x004400]
Dim $activeColor[19] = [0x00EE00, 0x00EE00, 0x00EE00, 0xEEEE00, 0xEEEE00, 0xEEEE00, 0xEE0000, 0xEE0000, 0xEE0000, 0x000000, 0xEE0000, 0xEE0000, 0xEE0000,  0xEEEE00, 0xEEEE00, 0xEEEE00, 0x00EE00, 0x00EE00, 0x00EE00]

$homepage = "http://192.168.0.15/"
$welcomePage = "<font face='arial' color='blue'><center><b>Welcome to the University of Michigan Mars Rover Control Panel</b><br>This page will automatically reload once you move the joystick or you can manually control by clicking the coordinate graph or typing commands into the URL bar and clicking 'Go'"
$title = "UMich Mars Rover Control Station"
GUICreate($title, 800, 550)


GUICtrlCreateGroup("Steering", 5, 5, 215, 540)
; LED array
For $i = 9 to -9 step -1
	$leftLabel[$i + 9] = GUICtrlCreateLabel("", 15, 210 - 20*$i, 97, 20, 0x01)
		GUICtrlSetTip($leftLabel[$i + 9], "About " & Round($i * 11 / 10)*10 & "% power", "Left motor power indicators")
		If($i <> 9) Then GUICtrlCreateLabel("", 15, 210 - 20*$i, 97, 2, 0x1000)
	$rightLabel[$i + 9] = GUICtrlCreateLabel("", 114, 210 - 20*$i, 97, 20, 0x01)
		GUICtrlSetTip($rightLabel[$i + 9], "About " & Round($i * 11 / 10)*10 & "% power", "Right motor power indicators")
		If($i <> 9) Then GUICtrlCreateLabel("", 114, 210 - 20*$i, 97, 2, 0x1000)
Next

; Motor labels
$leftMotorLabel = GUICtrlCreateLabel("Left Motor", 17, 213, 93, 16, 0x1)
	GUICtrlSetColor($leftMotorLabel, 0xFFFFFF)
	GUICtrlSetBkColor($leftMotorLabel, 0x000000)
$rightMotorLabel = GUICtrlCreateLabel("Right Motor", 115, 213, 93, 16, 0x1)
	GUICtrlSetColor($rightMotorLabel, 0xFFFFFF)
	GUICtrlSetBkColor($rightMotorLabel, 0x000000)
GUICtrlCreateLabel("", 112, 213, 3, 16, 0x1000)

; Checkboxes and options
$snappingCheckbox = GUICtrlCreateCheckbox("Snap", 15, 434, 45, 20)
	GUICtrlSetTip($snappingCheckbox, "Removes joystick jitters - Toggle with joystick button #2", "Makes it easier to drive the rover straight")
	GUICtrlSetState($snappingCheckbox, $GUI_CHECKED)
$snappingInput = GUICtrlCreateInput("10", 65, 434, 35, 20)
	GUICtrlSetTip($snappingInput, "Default 10px", "The X/Y offset that will be set to 0")
$snappingUpDown = GUICtrlCreateUpdown($snappingInput)
	GUICtrlSetTip($snappingUpDown, "Default 10px", "The X/Y offset that will be set to 0")
	GUICtrlSetLimit($snappingUpDown, 50, 0)
$lockCheckbox = GUICtrlCreateCheckbox("Lock input", 15, 455)
	GUICtrlSetTip($lockCheckbox, "Cruise control - Toggle with joystick trigger", "Prevents the joystick from controlling the rover")
$absoluteXYLabel = GUICtrlCreateLabel("Raw X:" & "" & "   Y:" & "", 10, 500, 95, 20, 0x02)
	GUICtrlSetTip($absoluteXYLabel, "Red crosshair", "This position of the joystick before any manipulations")
	GUICtrlSetColor($absoluteXYLabel, 0xFF0000)
$XYLabel = GUICtrlCreateLabel("X:" & "" & "   Y:" & "", 10, 523, 95, 20, 0x02)
	GUICtrlSetTip($XYLabel, "Black crosshair", "This position of the joystick after snapping is applied")

; Crosshair graph
$borderLabel = GUICtrlCreateLabel("", 109, 434, 102, 102)
	GUICtrlSetBkColor($borderLabel, 0x000000)
$fillLabel = GUICtrlCreateLabel("", 110, 435, 100, 100)
	GUICtrlSetBkColor($fillLabel, 0xFFFFFF)
$gridSnapLabel2 = GUICtrlCreateLabel("", 110 + 50 - 5, 435, 1 + 10, 100)
	GUICtrlSetBkColor($gridSnapLabel2, 0xDDDDDD)
$gridSnapLabel1 = GUICtrlCreateLabel("", 110, 435 + 50 - 5, 100, 1 + 10)
	GUICtrlSetBkColor($gridSnapLabel1, 0xDDDDDD)
$gridLabel2 = GUICtrlCreateLabel("", 110 + 50, 435, 1, 100)
	GUICtrlSetBkColor($gridLabel2, 0xAAAAAA)
$gridLabel1 = GUICtrlCreateLabel("", 110, 435 + 50, 100, 1)
	GUICtrlSetBkColor($gridLabel1, 0xAAAAAA)
$absoluteCrosshair = GUICtrlCreateLabel("+", 110 + 46 - 1, 435 + 46 - 4, 10, 10, 0x01)
	GUICtrlSetBkColor($absoluteCrosshair, $GUI_BKCOLOR_TRANSPARENT)
	GUICtrlSetColor($absoluteCrosshair, 0xFF0000)
$crosshair = GUICtrlCreateLabel("+", 110 + 46 - 1, 435 + 46 - 4, 10, 10, 0x01)
	GUICtrlSetBkColor($crosshair, $GUI_BKCOLOR_TRANSPARENT)

GUICtrlCreateGroup("Communication", 230, 5, 560, 540)
$oIE = _IECreateEmbedded() ; Create a simple communication with Internet Explorer
$homeButton = GUICtrlCreateButton("Home", 240, 25, 75, 20)
$urlInput = GUICtrlCreateInput($homepage, 320, 25, 380, 20)
$goButton = GUICtrlCreateButton("Go", 705, 25, 75, 20)
$GUIActiveX = GUICtrlCreateObj($oIE, 240, 50, 540, 485)
_IENavigate($oIE, "about:blank")
_IEBodyWriteHTML($oIE, $welcomePage)

InetGet("http://192.168.0.15/")

$coord = _GetJoy($joy,0)
$z = $coord[2]
For $i = 0 to 2
	$coord[$i] = convertCoord(Round(Sqrt($coord[$i])))
Next
GUISetState()

While 1
    $msg = GUIGetMsg()
	If($count >= $history - 1) Then
		$count = 0
	Else
		$count+=1
	EndIf
	If($msg == $homeButton) Then
		_IENavigate($oIE, $homepage)
	EndIf
	If($msg == $goButton) Then
		_IENavigate($oIE, GUICtrlRead($URLInput))
	EndIf
    $coord = _GetJoy($joy,0)
	If($buttonPress) Then
		If($coord[7] == 0) Then
			$buttonPress = False
		Else
			$coord[7] = 0;
		EndIf
	EndIf
	If(abs($coord[2] - $z) > 100 And Round(convertCoord(Sqrt($coord[2]))/255*50) <> GUICtrlRead($snappingInput)) Then
		GUICtrlSetData($snappingInput, Round(convertCoord(Sqrt($coord[2]))/255*50))
		$msg = $snappingInput
	EndIf
	For $i = 0 to 2
		$coord[$i] = convertCoord(Round(Sqrt($coord[$i])))
	Next
	$xValue = $coord[0]
	$yValue = $coord[1]
	If($coord[7] == 1) Then
		ControlClick($title, "", $lockCheckbox)
		$buttonPress = True
	EndIf
	If($coord[7] == 2) Then
		ControlClick($title, "", $snappingCheckbox)
		$buttonPress = True
	EndIf
	If($msg == $lockCheckbox) Then
		If(GUICtrlRead($lockCheckbox) == 1) Then
			$locked = True
			$pos = WinGetCaretPos()
			$pos2 = ControlGetPos($title, "", $lockCheckbox)
			ToolTip("Press the trigger to unlock", $pos[0] + $pos2[0] + 20, $pos[1] + $pos2[1] + 20, "You just locked the joystick", 1, 1)
			AdlibRegister("HideToolTip", 3000)
		Else
			$locked = False
			ToolTip('')
		EndIf
	EndIf
	If($msg == $snappingInput or $msg == $snappingCheckbox) Then
		If(GUICtrlRead($snappingCheckbox) == 1) Then
			GUICtrlSetPos($gridSnapLabel1, 110, 435 + 50 - GUICtrlRead($snappingInput) / 2, 100, 1 + GUICtrlRead($snappingInput))
			GUICtrlSetPos($gridSnapLabel2, 110 + 50 - GUICtrlRead($snappingInput) / 2, 435, 1 + GUICtrlRead($snappingInput), 100)
		Else
			GUICtrlSetPos($gridSnapLabel1, 110, 435 + 50, 100, 1)
			GUICtrlSetPos($gridSnapLabel2, 110 + 50, 435, 1, 100)
		EndIf
	EndIf

	$snapping = GUICtrlRead($snappingInput)
	If(GUICtrlRead($snappingCheckbox) <> 1) Then $snapping = 0
	If(abs($xValue-127) <= $snapping * 1.45) Then $xValue = 127
	If(abs($yValue-127) <= $snapping * 1.45) Then $yValue = 127
	$xValue = round($xValue / 255 * 200) - 100
	$yValue = round($yValue / 255 * 200) - 100
	$absoluteXValue = round($coord[0] / 255 * 200) - 100
	$absoluteYValue = round($coord[1] / 255 * 200) - 100
	$left = ($xValue - $yValue)
	$right = (0 - $xValue - $yValue)
	If($left > 100) Then $left = 100
	If($right > 100) Then $right = 100
	If($left < -100) Then $left = -100
	If($right < -100) Then $right = -100

	$left = Round($left / 200 * 18) + 9
	$right = Round($right / 200 * 18) + 9
	$oldLeft[$count] = $left
	$oldRight[$count] = $right

	GUICtrlSetPos($crosshair, 110 + 45 + Round($xValue / 255 * 120), 435 + 43 + Round($yValue / 255 * 120))
	GUICtrlSetPos($absoluteCrosshair, 110 + 45 + Round($absoluteXValue / 255 * 120), 435 + 43 + Round($absoluteYValue / 255 * 120))
	If(GUICtrlRead($absoluteXYLabel) <> "Raw X:" & $absoluteXValue & "   Y:" & -1*$absoluteYValue) Then GUICtrlSetData($absoluteXYLabel, "Raw X:" & $absoluteXValue & "   Y:" & -1*$absoluteYValue)
	If(GUICtrlRead($XYLabel) <> "X:" & $xValue & "   Y:" & -1*$yValue) Then GUICtrlSetData($XYLabel, "X:" & $xValue & "   Y:" & -1*$yValue)

	$updateLeft = true
	For $i = 0 to $history - 1
		For $j = 0 to $history - 1
			If($oldLeft[$i] <> $oldLeft[$j]) Then $updateLeft = false
		Next
	Next

	$updateRight = true
	For $i = 0 to $history - 1
		For $j = 0 to $history - 1
			If($oldRight[$i] <> $oldRight[$j]) Then $updateRight = false
		Next
	Next

	If($updateLeft And Not $locked) Then
		If($oldLeft[0] <> $oldLeft[$history]) Then
			If($left < 9) Then
				For $i = 0 to $left
					If($leftActive[$i] == 1) Then GUICtrlSetBkColor($leftLabel[$i], $inactiveColor[$i])
					$leftActive[$i] = 0
				Next
				For $i = $left to 8
					If($leftActive[$i] == 0) Then GUICtrlSetBkColor($leftLabel[$i], $activeColor[$i])
					$leftActive[$i] = 1
				Next
				For $i = 10 to 18
					If($leftActive[$i] == 1) Then GUICtrlSetBkColor($leftLabel[$i], $inactiveColor[$i])
					$leftActive[$i] = 0
				Next
			ElseIf($left > 9) Then
				For $i = 0 to 8
					If($leftActive[$i] == 1) Then GUICtrlSetBkColor($leftLabel[$i], $inactiveColor[$i])
					$leftActive[$i] = 0
				Next
				For $i = 10 to $left
					If($leftActive[$i] == 0) Then GUICtrlSetBkColor($leftLabel[$i], $activeColor[$i])
					$leftActive[$i] = 1
				Next
				For $i = $left + 1 to 18
					If($leftActive[$i] == 1) Then GUICtrlSetBkColor($leftLabel[$i], $inactiveColor[$i])
					$leftActive[$i] = 0
				Next
			Else
				For $i = 0 to 18
					If($leftActive[$i] == 1) Then GUICtrlSetBkColor($leftLabel[$i], $inactiveColor[$i])
					$leftActive[$i] = 0
				Next
			EndIf
			$oldLeft[$history] = $oldLeft[0]
		EndIf
	EndIf

	If($updateRight And Not $locked) Then
		If($oldRight[0] <> $oldRight[$history]) Then
			If($right < 9) Then
				For $i = 0 to $right
					If($rightActive[$i] == 1) Then GUICtrlSetBkColor($rightLabel[$i], $inactiveColor[$i])
					$rightActive[$i] = 0
				Next
				For $i = $right to 8
					If($rightActive[$i] == 0) Then GUICtrlSetBkColor($rightLabel[$i], $activeColor[$i])
					$rightActive[$i] = 1
				Next
				For $i = 10 to 18
					If($rightActive[$i] == 1) Then GUICtrlSetBkColor($rightLabel[$i], $inactiveColor[$i])
					$rightActive[$i] = 0
				Next
			ElseIf($right > 9) Then
				For $i = 0 to 8
					If($rightActive[$i] == 1) Then GUICtrlSetBkColor($rightLabel[$i], $inactiveColor[$i])
					$rightActive[$i] = 0
				Next
				For $i = 10 to $right
					If($rightActive[$i] == 0) Then GUICtrlSetBkColor($rightLabel[$i], $activeColor[$i])
					$rightActive[$i] = 1
				Next
				For $i = $right + 1 to 18
					If($rightActive[$i] == 1) Then GUICtrlSetBkColor($rightLabel[$i], $inactiveColor[$i])
					$rightActive[$i] = 0
				Next
			Else
				For $i = 0 to 18
					If($rightActive[$i] == 1) Then GUICtrlSetBkColor($rightLabel[$i], $inactiveColor[$i])
					$rightActive[$i] = 0
				Next
			EndIf
			$oldRight[$history] = $oldRight[0]
		EndIf
	EndIf

    If $msg = $GUI_EVENT_CLOSE Then Exitloop
WEnd

DllClose("Winmm.dll")

Func convertCoord($value)
	$y = Floor(10^(2.002*Log($value)/Log(10) - 2.413))
	If $y > 255 Then $y = 255
	If $y < 0 Then $y = 0
	Return $y
EndFunc

;======================================
;   _JoyInit()
;======================================
Func _JoyInit()
    Local $joy
    Global $JOYINFOEX_struct    = "dword[13]"

    $joy=DllStructCreate($JOYINFOEX_struct)
    if @error Then Return 0
    DllStructSetData($joy, 1, DllStructGetSize($joy), 1);dwSize = sizeof(struct)
    DllStructSetData($joy, 1, 255, 2)              ;dwFlags = GetAll
    return $joy
EndFunc

;======================================
;   _GetJoy($lpJoy,$iJoy)
;   $lpJoy  Return from _JoyInit()
;   $iJoy   joystick # 0-15
;   Return  Array containing X-Pos, Y-Pos, Z-Pos, R-Pos, U-Pos, V-Pos,POV
;           Buttons down
;
;           *POV This is a digital game pad, not analog joystick
;           65535   = Not pressed
;           0       = U
;           4500    = UR
;           9000    = R
;           Goes around clockwise increasing 4500 for each position
;======================================
Func _GetJoy($lpJoy,$iJoy)
    Local $coor,$ret

    Dim $coor[8]
    DllCall("Winmm.dll","int","joyGetPosEx", _
            "int",$iJoy, _
            "ptr",DllStructGetPtr($lpJoy))

    if Not @error Then
        $coor[0]    = DllStructGetData($lpJoy,1,3)
        $coor[1]    = DllStructGetData($lpJoy,1,4)
        $coor[2]    = DllStructGetData($lpJoy,1,5)
        $coor[3]    = DllStructGetData($lpJoy,1,6)
        $coor[4]    = DllStructGetData($lpJoy,1,7)
        $coor[5]    = DllStructGetData($lpJoy,1,8)
        $coor[6]    = DllStructGetData($lpJoy,1,11)
        $coor[7]    = DllStructGetData($lpJoy,1,9)
    EndIf

    return $coor
EndFunc

Func HideToolTip()
	ToolTip('')
	AdlibUnRegister()
EndFunc